import { Detector } from "./Detector";
import { DetectorResult } from "../DetectorResult";
import { Geometry } from "../../geometry/Geometry";
import { Transform } from "../../../common/Transform";
import { CachedDetectorData } from "./CachedDetectorData";
import { Vec3 } from "../../../common/Vec3";
import { M } from "../../../common/M";
import { MathUtil } from "../../../common/MathUtil";
import { SphereGeometry } from "../../geometry/SphereGeometry";

/**
 * Sphere vs Sphere detector.
 */
export class SphereSphereDetector extends Detector {
	/**
	 * Default constructor.
	 */
	constructor() {
		super(false);
	}

	public detectImpl(result: DetectorResult, geom1: Geometry, geom2: Geometry, tf1: Transform, tf2: Transform, cachedData: CachedDetectorData): void {
		var s1: SphereGeometry = geom1 as SphereGeometry;
		var s2: SphereGeometry = geom2 as SphereGeometry;

		result.incremental = false;

		var d: Vec3 = new Vec3();
		M.vec3_sub(d, tf1._position, tf2._position);
		var r1: number = s1._radius;
		var r2: number = s2._radius;
		var len2: number = M.vec3_dot(d, d);
		if (len2 >= (r1 + r2) * (r1 + r2)) return;
		var len: number = MathUtil.sqrt(len2);
		var n: Vec3 = new Vec3();
		if (len > 0) {
			M.vec3_scale(n, d, 1 / len);
		} else {
			n.init(1, 0, 0);
		}
		this.setNormal(result, n);
		var pos1: Vec3 = new Vec3();
		var pos2: Vec3 = new Vec3();
		M.vec3_addRhsScaledTo(pos1, tf1._position, n, -r1);
		M.vec3_addRhsScaledTo(pos2, tf2._position, n, r2);
		this.addPoint(result, pos1, pos2, r1 + r2 - len, 0);
	}
}
